#include "w25qxx.h"
#include "spi.h"
#define __DEBUG__ 	0
#if __DEBUG__  
#include "uart.h"
    #define DEBUG(format,...)    printf(""format"", ##__VA_ARGS__)  

#else
    #define DEBUG(format,...)  
#endif

#define DUMMY				0xa5
#define W25QXX_SPI			SPI5
#define w25Qxx_cs_clr()		do{LL_GPIO_ResetOutputPin(GPIOF, LL_GPIO_PIN_6);}while(0)
#define w25Qxx_cs_set()		do{LL_GPIO_SetOutputPin(GPIOF, LL_GPIO_PIN_6);}while(0)
#define w25Qxx_led_on()		do{LL_GPIO_ResetOutputPin(GPIOD, LL_GPIO_PIN_11);}while(0)
#define w25Qxx_led_off()	do{LL_GPIO_SetOutputPin(GPIOD, LL_GPIO_PIN_11);}while(0)
static uint8_t w25QxxBuf[W25QXX_SECTOR_SIZE];

static uint8_t w25Qxx_spi(uint8_t data) {
	uint8_t ret;
	while(LL_SPI_IsActiveFlag_TXE(W25QXX_SPI) == RESET);
	LL_SPI_TransmitData8(W25QXX_SPI, data);
	while(LL_SPI_IsActiveFlag_RXNE(W25QXX_SPI) == RESET);
	ret = LL_SPI_ReceiveData8(W25QXX_SPI);
	return ret;
}

static uint8_t W25QXX_ReadStatusReg(void) {
	uint8_t value = 0;
	w25Qxx_cs_clr();
	w25Qxx_spi(READ_STATUS_REG);
	value = w25Qxx_spi(DUMMY);
	w25Qxx_cs_set();
	return value;
}

static void W25QXX_WaitBusy(void) {
	while(1) {
		if((W25QXX_ReadStatusReg() & 0x01) == 0) {
			break;
		}
	}
}

static void W25QXX_WriteEnable(void) {
	w25Qxx_cs_clr();
	w25Qxx_spi(WRITE_ENABLE);
	w25Qxx_cs_set();
	while(1) {
		if((W25QXX_ReadStatusReg() & 0x02) != RESET) {
			break;
		}
	}
}

void W25QXX_EraseSector(uint32_t address) {
	W25QXX_WriteEnable();
	w25Qxx_cs_clr();
	w25Qxx_spi(SECTOR_ERASE_4K);
	w25Qxx_spi((uint8_t)((address)>>16));  
    w25Qxx_spi((uint8_t)((address)>>8));   
    w25Qxx_spi((uint8_t)address);
	w25Qxx_cs_set();
	W25QXX_WaitBusy();
}

static uint8_t W25QXX_ReadID(void) {
	uint8_t id = 0;
	w25Qxx_cs_clr();
	w25Qxx_spi(READ_DEVICE_ID);
	w25Qxx_spi(DUMMY);
	w25Qxx_spi(DUMMY);
	w25Qxx_spi(0x00);
	w25Qxx_spi(DUMMY);
	id = w25Qxx_spi(DUMMY);
	w25Qxx_cs_set();
	return id;
}

void W25QXX_Read(uint32_t address, uint8_t *pBuf, uint32_t length) {
	uint32_t index;
	w25Qxx_led_off();
	w25Qxx_cs_clr();
	w25Qxx_spi(READ_DATA);
	w25Qxx_spi((uint8_t)((address)>>16));  
    w25Qxx_spi((uint8_t)((address)>>8));   
    w25Qxx_spi((uint8_t)address);
	for(index=0;index<length;index++) {
		pBuf[index] = w25Qxx_spi(DUMMY);
	}
	w25Qxx_cs_set();
	w25Qxx_led_on();
}

static void W25QXX_PageProgram(uint32_t address, uint8_t *pBuf, uint16_t length) {
	uint16_t index;
	W25QXX_WriteEnable();
	w25Qxx_led_off();
	w25Qxx_cs_clr();
	w25Qxx_spi(PAGE_PROGRAM);
	w25Qxx_spi((uint8_t)((address)>>16));  
    w25Qxx_spi((uint8_t)((address)>>8));   
    w25Qxx_spi((uint8_t)address);
	for(index=0;index<length;index++) {
		w25Qxx_spi(pBuf[index]);
	}
	w25Qxx_cs_set();
	W25QXX_WaitBusy();
	w25Qxx_led_on();
}

void W25QXX_WriteWithNoCheck(uint32_t address, uint8_t *pBuf, uint32_t length) {
	uint32_t curAddress, remain, curWriteCnt;
	curAddress = address;
	remain = length;
	while(1) {
		if(curAddress % W25QXX_PAGE_SIZE) {
			curWriteCnt = W25QXX_PAGE_SIZE - curAddress % W25QXX_PAGE_SIZE;
		}
		else {
			curWriteCnt = W25QXX_PAGE_SIZE;
		}
		if(remain <= curWriteCnt)
			curWriteCnt = remain;
		W25QXX_PageProgram(curAddress, pBuf, curWriteCnt);
		remain = remain - curWriteCnt;
		if(remain) {
			pBuf += curWriteCnt;
			curAddress += curWriteCnt;
		}
		else 
			break;
	}
}

void W25QXX_Write(uint32_t address, uint8_t *pBuf, uint32_t length) {
	uint32_t curAddress, remain, curWriteCnt;
	uint16_t index;
	curAddress = address;
	remain = length;
	DEBUG("W25QXX: Starting to write %d datas to device.\n", remain);
	while(1) {
		if(curAddress % W25QXX_SECTOR_SIZE) {
			curWriteCnt = W25QXX_SECTOR_SIZE - curAddress % W25QXX_SECTOR_SIZE;
		}
		else {
			curWriteCnt = W25QXX_SECTOR_SIZE;
		}
		if(remain <= curWriteCnt)
			curWriteCnt = remain;
		DEBUG("W25QXX: Current write address 0x%08x\n", curAddress);
		DEBUG("W25QXX: Current write length %d\n", curWriteCnt);
		DEBUG("W25QXX: Current write sector address 0x%08x\n", curAddress-curAddress%W25QXX_SECTOR_SIZE);
		DEBUG("W25QXX: Current sector's details:\n");
		W25QXX_Read((curAddress-curAddress%W25QXX_SECTOR_SIZE), w25QxxBuf, W25QXX_SECTOR_SIZE);
		for(index = 0; index < W25QXX_SECTOR_SIZE;) {
			if(index%W25QXX_PAGE_SIZE == 0)
				DEBUG("Page %02d -> ", index/W25QXX_PAGE_SIZE);
			DEBUG("%02x ", w25QxxBuf[index]);
			index++;
			if(index%W25QXX_PAGE_SIZE == 0)
				DEBUG("\n");
		}
		for(index = 0; index < curWriteCnt; index++) {
			if(w25QxxBuf[curAddress%W25QXX_SECTOR_SIZE + index] != 0xFF)
				break;
		}
		if(index != curWriteCnt) {
			DEBUG("W25QXX: Need to erase sector. address = 0x%08x index = %d value = 0x%02x\n", 
				curAddress-curAddress%W25QXX_SECTOR_SIZE, 
				curAddress%W25QXX_SECTOR_SIZE+index, 
				w25QxxBuf[curAddress%W25QXX_SECTOR_SIZE+index]);
			W25QXX_EraseSector(curAddress-curAddress%W25QXX_SECTOR_SIZE);
			for(index = 0; index < curWriteCnt; index++) {
				w25QxxBuf[curAddress % W25QXX_SECTOR_SIZE + index] = pBuf[index];
			}
			W25QXX_WriteWithNoCheck(curAddress-curAddress%W25QXX_SECTOR_SIZE, w25QxxBuf, W25QXX_SECTOR_SIZE);
		}
		else {
			DEBUG("W25QXX: Ready to write data.\n");
			W25QXX_WriteWithNoCheck(curAddress, pBuf, curWriteCnt);
		}
		remain = remain - curWriteCnt;
		DEBUG("W25QXX: Remain = %d\n", remain);
		if(remain) {
			pBuf += curWriteCnt;
			curAddress += curWriteCnt;
		}
		else {
			break;
		}
	}
}

void W25QXX_Init(void) {
	uint8_t ID;
	DEBUG("SPI_FLASH_Init\n");
	//CS
	LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOF);
	LL_GPIO_SetPinMode(GPIOF, LL_GPIO_PIN_6, LL_GPIO_MODE_OUTPUT);
	LL_GPIO_SetPinOutputType(GPIOF, LL_GPIO_PIN_6, LL_GPIO_OUTPUT_PUSHPULL);
	LL_GPIO_SetPinPull(GPIOF, LL_GPIO_PIN_6, LL_GPIO_PULL_UP);
	LL_GPIO_SetPinSpeed(GPIOF, LL_GPIO_PIN_6, LL_GPIO_SPEED_FREQ_VERY_HIGH);
	LL_GPIO_SetAFPin_0_7(GPIOF, LL_GPIO_PIN_6, LL_GPIO_AF_0);
	w25Qxx_cs_set();
	//W25QXX_IndicationLED_Config
	LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOD);
	LL_GPIO_SetPinMode(GPIOD, LL_GPIO_PIN_11, LL_GPIO_MODE_OUTPUT);
	LL_GPIO_SetPinOutputType(GPIOD, LL_GPIO_PIN_11, LL_GPIO_OUTPUT_PUSHPULL);
	LL_GPIO_SetPinPull(GPIOD, LL_GPIO_PIN_11, LL_GPIO_PULL_UP);
	LL_GPIO_SetPinSpeed(GPIOD, LL_GPIO_PIN_11, LL_GPIO_SPEED_FREQ_VERY_HIGH);
	LL_GPIO_SetAFPin_8_15(GPIOD, LL_GPIO_PIN_11, LL_GPIO_AF_0);
	w25Qxx_led_on();
	SPI5_Init();
	ID = W25QXX_ReadID();
	if(ID != 0x17) {
		printf("ID = %02X \n", ID);
		printf("There is no nor flash chip.\n");
	}
	else {
		printf("Identify the chip W25Q128.\n");
	}
}

